///@file ElastoplasticMaterial.cpp
///@brief Implementation of the class representing a linear elastic material with isotropic hardening plastic behaviour
///@author Arnaud Duval
///@version 0.0
///@date 2011/03/10

#include <cmath>

#include "ElastoplasticMaterial.h"
#include "StressVector.h"

#define DEBUG_ELASTOPLASTIC_MATERIAL false

///@brief Default constructor
ElastoplasticMaterial::ElastoplasticMaterial()
{
    young = 0.;
    poisson = 0.;
    Hiso = 0.;
    n = 0.;
    sigmaY = 0.;
}

///@brief Destructor
ElastoplasticMaterial::~ElastoplasticMaterial()
{

}

///@brief Constructor with arguments
///@param Dyoung given Young modulus
///@param Dpoisson given Poisson ratio
///@param DHiso given first isotropic hardening coefficient
///@param Dn given second isotropic hardening coefficient
///@param DsigmaY given Yield stress
ElastoplasticMaterial::ElastoplasticMaterial(double Dyoung, double Dpoisson, double DHiso, double Dn, double DsigmaY)
{
    young = Dyoung;
    poisson = Dpoisson;
    Hiso = DHiso;
    n = Dn;
    sigmaY = DsigmaY;
}

///@brief Set the value of a given parameter
///@param idx Index of parameter to set (1 for Young modulus, 2 for Poisson ratio, 3 for first isotropic hardening coefficient, 4
///for second isotropic hardening coefficient , 5 for Yield strass)
///@param value Value to set to parameter
void ElastoplasticMaterial::SetParameter(int idx, double value)
{
    assert((idx >= 1)||(idx <= 5));
    switch(idx)
    {
        case 1:
        {
            young = value;
        }break;
        case 2:
        {
            poisson = value;
        }break;
        case 3:
        {
            Hiso = value;
        }
        case 4:
        {
            n = value;
        }
        case 5:
        {
            sigmaY = value;
        }
    }
}

///@brief Compute stess tensor increment from strain tensor tensor increment and initial stress and strain
///@param Eini Strain tensor at the beginning of increment
///@param Sini Stress tensor at the beginning of increment
///@param DE Strain increment
StressTensor ElastoplasticMaterial::Dstress(StrainTensor Eini, StressTensor Sini, StrainTensor DE)
{
    StressTensor res;

    ///TODO

    return res;
}

///@brief Print output
///@param s out stream
void ElastoplasticMaterial::PrintOutput(std::ostream &s)
{
    s << reference.c_str() << "\t" << "Elastic\tE = " << young << "\tnu = " << poisson
      << "\tHiso = " << Hiso << "\tn = " << n << "\tsigmaY = " << sigmaY;
}

///@brief Similar to an Abaqus UMAT
///@warning The name of this function should be changed
///@return true if convergence is OK
///@param strain Strain at the beginning of increment
///@param dstrain Increment of strain
///@param stress Stress at the beginning of increment. Should be updated
///@param ddsdde Tangent operator at the end of increment
///@param statev Array of evolution variables
///@param nstatev Number of evolution variables
///@warning This function should optimized with use of SymTensor, StressTensor and StrainTensor (needs classes for stiffness
/// tensors)
bool ElastoplasticMaterial::UMAT(const Vector& strain, const Vector& dstrain, Vector& stress, Matrix& ddsdde,
                                 double* statev,unsigned int nstatev)
{
    /*std::cout << "Appel UMAT\n";
    std::cout << "strain : " << strain << "\n";
    std::cout << "dstrain : " << dstrain << "\n";
    std::cout << "stress : " << stress << "\n";
    std::cout << "statev : ";
    for(unsigned int i = 0 ; i < nstatev ; i++)
        std::cout << statev[i] << "\t" ;
    std::cout << "\n";*/

    Vector stressini = stress;
    double *statevini = new double[nstatev];
    for(unsigned int i = 0 ; i < nstatev ; i++)
        statevini[i] = statev[i];

    
    bool OK = true;
    ///@warning only the 3D case is taken into account
    if(strain.Dim() == 6)
    {
        //Converting input data
        Tensor2 sigma(3u);
        Tensor2 epsilon(3u);
        Tensor2 Depsilon(3u);
        Tensor2 epsilonp(3u);
        double p;

        sigma(1,1) = stress[1];
        sigma(2,2) = stress[2];
        sigma(3,3) = stress[3];
        sigma(1,2) = stress[4];
        sigma(2,1) = stress[4];
        sigma(1,3) = stress[5];
        sigma(3,1) = stress[5];
        sigma(2,3) = stress[6];
        sigma(3,2) = stress[6];

        epsilon(1,1) = strain[1];
        epsilon(2,2) = strain[2];
        epsilon(3,3) = strain[3];
        epsilon(1,2) = strain[4]/2.;
        epsilon(2,1) = strain[4]/2.;
        epsilon(1,3) = strain[5]/2.;
        epsilon(3,1) = strain[5]/2.;
        epsilon(2,3) = strain[6]/2.;
        epsilon(3,2) = strain[6]/2.;

        Depsilon(1,1) = dstrain[1];
        Depsilon(2,2) = dstrain[2];
        Depsilon(3,3) = dstrain[3];
        Depsilon(1,2) = dstrain[4]/2.;
        Depsilon(2,1) = dstrain[4]/2.;
        Depsilon(1,3) = dstrain[5]/2.;
        Depsilon(3,1) = dstrain[5]/2.;
        Depsilon(2,3) = dstrain[6]/2.;
        Depsilon(3,2) = dstrain[6]/2.;

        p = statev[0];
        
        epsilonp(1,1) = statev[1];
        epsilonp(2,2) = statev[2];
        epsilonp(3,3) = statev[3];
        epsilonp(1,2) = statev[4];
        epsilonp(2,1) = statev[4];
        epsilonp(1,3) = statev[5];
        epsilonp(3,1) = statev[5];
        epsilonp(2,3) = statev[6];
        epsilonp(3,2) = statev[6];

        Tensor4 stiffness = StiffnessTensor(3);
        
        //Stress prediction
        Tensor2 Dsigma = (stiffness||Depsilon);
        Tensor2 sigmapred = sigma + Dsigma;
        double Dp = 0.;

        double f = this->LoadingFunction(sigmapred, p);

        if(f < 0.)
        {
            sigma = sigmapred;
            ddsdde.Nullify();
            double lambda = poisson*young/((1. + poisson)*(1. - 2.*poisson));
            double mu = young / (2.*(1. + poisson));
            ddsdde(1,1) = 2.*mu + lambda;
            ddsdde(2,2) = 2.*mu + lambda;
            ddsdde(3,3) = 2.*mu + lambda;
            ddsdde(1,2) = lambda;
            ddsdde(2,1) = lambda;
            ddsdde(1,3) = lambda;
            ddsdde(3,1) = lambda;
            ddsdde(2,3) = lambda;
            ddsdde(3,2) = lambda;
            ddsdde(4,4) = mu;
            ddsdde(5,5) = mu;
            ddsdde(6,6) = mu;
        }
        else
        {
            Dsigma.Nullify();
            Vector residual = ComputeResiduals(sigma, Dsigma, p, Dp, epsilon, Depsilon, epsilonp);
           
            bool conv = false;
            Matrix jacobian, invJac;
            //Newton-Raphson loop
            for(unsigned int counter = 1 ; (counter <= 100) && (!conv) ; counter++)
            {
                jacobian = ComputeResidualsDerivatives(sigma, Dsigma, p, Dp, epsilon, Depsilon, epsilonp);
                invJac = jacobian.Invert();
                Vector corr = invJac|residual;

                Dsigma(1,1) = Dsigma(1,1) - corr[1];
                Dsigma(2,2) = Dsigma(2,2) - corr[2];
                Dsigma(3,3) = Dsigma(3,3) - corr[3];
                Dsigma(1,2) = Dsigma(1,2) - corr[4];
                Dsigma(2,1) = Dsigma(2,1) - corr[4];
                Dsigma(1,3) = Dsigma(1,3) - corr[5];
                Dsigma(3,1) = Dsigma(3,1) - corr[5];
                Dsigma(2,3) = Dsigma(2,3) - corr[6];
                Dsigma(3,2) = Dsigma(3,2) - corr[6];
                Dp -= corr[7];
                if(Dp < 0.)
                    Dp = 0.;
                Vector prevRes = residual;
                residual = ComputeResiduals(sigma, Dsigma, p, Dp, epsilon, Depsilon, epsilonp);

                if((residual - prevRes).Euclide_Norm() < 1.e-8)
                    conv = true;
            }
            OK = conv;
            
            sigma = sigma + Dsigma;
            p = p + Dp;
            epsilonp = epsilonp + Dp*(3./2.)*sigma.Dev()/(sigma.VMEquivStress());

            ddsdde(1,1) = invJac(1,1);
            ddsdde(1,2) = invJac(1,2);
            ddsdde(1,3) = invJac(1,3);
            ddsdde(1,4) = invJac(1,4)*0.5;
            ddsdde(1,5) = invJac(1,5)*0.5;
            ddsdde(1,6) = invJac(1,6)*0.5;
            
            ddsdde(2,1) = invJac(2,1);
            ddsdde(2,2) = invJac(2,2);
            ddsdde(2,3) = invJac(2,3);
            ddsdde(2,4) = invJac(2,4)*0.5;
            ddsdde(2,5) = invJac(2,5)*0.5;
            ddsdde(2,6) = invJac(2,6)*0.5;

            ddsdde(3,1) = invJac(3,1);
            ddsdde(3,2) = invJac(3,2);
            ddsdde(3,3) = invJac(3,3);
            ddsdde(3,4) = invJac(3,4)*0.5;
            ddsdde(3,5) = invJac(3,5)*0.5;
            ddsdde(3,6) = invJac(3,6)*0.5;

            ddsdde(4,1) = invJac(4,1);
            ddsdde(4,2) = invJac(4,2);
            ddsdde(4,3) = invJac(4,3);
            ddsdde(4,4) = invJac(4,4)*0.5;
            ddsdde(4,5) = invJac(4,5)*0.5;
            ddsdde(4,6) = invJac(4,6)*0.5;

            ddsdde(5,1) = invJac(5,1);
            ddsdde(5,2) = invJac(5,2);
            ddsdde(5,3) = invJac(5,3);
            ddsdde(5,4) = invJac(5,4)*0.5;
            ddsdde(5,5) = invJac(5,5)*0.5;
            ddsdde(5,6) = invJac(5,6)*0.5;

            ddsdde(6,1) = invJac(6,1);
            ddsdde(6,2) = invJac(6,2);
            ddsdde(6,3) = invJac(6,3);
            ddsdde(6,4) = invJac(6,4)*0.5;
            ddsdde(6,5) = invJac(6,5)*0.5;
            ddsdde(6,6) = invJac(6,6)*0.5;

            if(Dp == 0.)
            {
                ddsdde.Nullify();
                double lambda = poisson*young/((1. + poisson)*(1. - 2.*poisson));
                double mu = young / (2.*(1. + poisson));
                ddsdde(1,1) = 2.*mu + lambda;
                ddsdde(2,2) = 2.*mu + lambda;
                ddsdde(3,3) = 2.*mu + lambda;
                ddsdde(1,2) = lambda;
                ddsdde(2,1) = lambda;
                ddsdde(1,3) = lambda;
                ddsdde(3,1) = lambda;
                ddsdde(2,3) = lambda;
                ddsdde(3,2) = lambda;
                ddsdde(4,4) = mu;
                ddsdde(5,5) = mu;
                ddsdde(6,6) = mu;
            }
        }

        statev[0] = p;
        statev[1] = epsilonp(1,1);
        statev[2] = epsilonp(2,2);
        statev[3] = epsilonp(3,3);
        statev[4] = epsilonp(1,2);
        statev[5] = epsilonp(1,3);
        statev[6] = epsilonp(2,3);

        stress[1] = sigma(1,1);
        stress[2] = sigma(2,2);
        stress[3] = sigma(3,3);
        stress[4] = sigma(1,2);
        stress[5] = sigma(1,3);
        stress[6] = sigma(2,3);
    }
    else if(strain.Dim() == 3)
    {
        ///@warning Only the plane stress case is take into account
        //Converting input data
        StressVector sigma(3u);
        Vector epsilon(3u);
        Vector Depsilon(3u);
        Vector epsilonp(3u);
        double p;

        sigma = stress;
        epsilon= strain;
        Depsilon = dstrain;
        p = statev[0];

        epsilonp[1] = statev[1];
        epsilonp[2] = statev[2];
        epsilonp[3] = statev[3];


        Matrix stiffness = StiffnessMatrix(2);

        //Stress prediction
        StressVector Dsigma;
        Dsigma = (stiffness|Depsilon);
        StressVector sigmapred = sigma + Dsigma;
        if(DEBUG_ELASTOPLASTIC_MATERIAL)
        {
            std::cout<< "Prediction\n";
            std::cout << "sigma : " << sigma << "\n";
            std::cout << "Dsigma : " << Dsigma << "\n";
        }

        double f = this->LoadingFunction(sigmapred, p);
        double Dp = 0.;


        if((f < 0.)||(Depsilon.Euclide_Norm() == 0.))
        {
            if(DEBUG_ELASTOPLASTIC_MATERIAL)
                std::cout << "ELASTIQUE OU PAS D'INCREMENT\n";
            sigma = sigmapred;
            ddsdde = StiffnessMatrix(2);
        }
        else
        {
            if(DEBUG_ELASTOPLASTIC_MATERIAL)
                std::cout << "PLASTIQUE\n";
            //Test d'un changement de sens
            if((sigma | Dsigma) < 0.)
            {
                if(DEBUG_ELASTOPLASTIC_MATERIAL)
                    std::cout << "detection d'un changement de sens\n";
                Dsigma = ((Dsigma.VMEquiv() - f)/(Dsigma.VMEquiv()))*Dsigma;
            }
            else
                Dsigma.Nullify();
            Vector residual = ComputeResiduals(sigma, Dsigma, p, Dp, epsilon, Depsilon, epsilonp);

            bool conv = false;
            Matrix jacobian, invJac;
            //Newton-Raphson loop
            for(unsigned int counter = 1 ; (counter <= 100) && (!conv) ; counter++)
            {
                //std::cout << "res : " << residual << "\n";
                jacobian = ComputeResidualsDerivatives(sigma, Dsigma, p, Dp, epsilon, Depsilon, epsilonp);
                //std::cout << "JACOB : \n" << jacobian << "\n\n";
                invJac = jacobian.Invert();
                //std::cout << "INVERSE \n" << invJac << "\n\n";
                Vector corr = invJac|residual;
                //std::cout << "corr : " << corr << "\n";

                Dsigma[1] = Dsigma[1] - corr[1];
                Dsigma[2] = Dsigma[2] - corr[2];
                Dsigma[3] = Dsigma[3] - corr[3];
                Dp -= corr[4];
                if(Dp < 0.)
                {
                    Dp = 0.;
                }
                if(DEBUG_ELASTOPLASTIC_MATERIAL)
                {
                    std::cout << "Après correction : \t";
                    std::cout << "sigma + Dsigma : " << sigma + Dsigma << "\t";
                    std::cout << "p + Dp : " << p + Dp << "\n";
                }
                
                Vector prevRes = residual;
                residual = ComputeResiduals(sigma, Dsigma, p, Dp, epsilon, Depsilon, epsilonp);

                /*if((residual - prevRes).Euclide_Norm() < 1.e-8)
                    conv = true;*/
                /*if(corr.Euclide_Norm() < 1.e-8)
                    conv = true;*/
                //std::cout << "sigma + dsigma : " << sigma + Dsigma << "\n";

                //Essai pour calculer une erreur
                double error = 0.;
                //Equilibrage de la surface de charge par rapport au niveau de contrainte
                //Faire attention à la division par zero
                error += fabs(residual[4])/((sigma + Dsigma).VMEquiv());
                //std::cout << "contribution f : " << fabs(residual[4]) << "\t" << ((sigma + Dsigma).VMEquiv()) << "\n";

                //Equilibrage des déformation par rapport au niveau de deformation
                //std::cout << "contribution eps : " << (fabs(residual[1]) + fabs(residual[2]) + fabs(residual[3])) << "\t"
                //          << ((epsilon+Depsilon).Euclide_Norm()) << "\n";
                error += (fabs(residual[1]) + fabs(residual[2]) + fabs(residual[3]))/((epsilon+Depsilon).Euclide_Norm());

                if(DEBUG_ELASTOPLASTIC_MATERIAL)
                    std::cout << counter << "   Error : " << error << "\n";

                if(error < 1.e-3)
                {
                    conv = true;
                }

            }
            OK = conv;

            //On recolle un coup de calcul des dérivées
            jacobian = ComputeResidualsDerivatives(sigma, Dsigma, p, Dp, epsilon, Depsilon, epsilonp);
            invJac = jacobian.Invert();

            sigma = sigma + Dsigma;
            p = p + Dp;
            Matrix D(3u, 'D');
            Matrix P2(3u, 'P');
            epsilonp = epsilonp + Dp*(3./2.)*(P2|(D|sigma))/(sigma.VMEquiv());

            ddsdde(1,1) = invJac(1,1);
            ddsdde(1,2) = invJac(1,2);
            ddsdde(1,3) = invJac(1,3);

            ddsdde(2,1) = invJac(2,1);
            ddsdde(2,2) = invJac(2,2);
            ddsdde(2,3) = invJac(2,3);

            ddsdde(3,1) = invJac(3,1);
            ddsdde(3,2) = invJac(3,2);
            ddsdde(3,3) = invJac(3,3);

            if(Dp == 0.)
            {
                ddsdde = StiffnessMatrix(2);
            }
        }

        statev[0] = p;
        statev[1] = epsilonp[1];
        statev[2] = epsilonp[2];
        statev[3] = epsilonp[3];

        stress = sigma;
    }

    if(!OK)
    {
        std::cerr << "Convergence error in ElastoplasticMaterial::UMAT\n";
        if(DEBUG_ELASTOPLASTIC_MATERIAL)
        {
            std::cout << "Convergence error in ElastoplasticMaterial::UMAT\n";
            std::cout << "Appel UMAT(strain, dstrain, stress, ddsdde, statev, 4) : \n";
            std::cout << "strain : " << strain << "\n";
            std::cout << "dstrain : " << dstrain << "\n";
            std::cout << "stress : " << stressini << "\n";
            std::cout << "statev : ";
            for(unsigned int i = 0 ; i < 4 ; i++)
                std::cout << statev[i] << "\t";
            std::cout << "\n";
        }
        

    }

    /*std::cout << "Fin UMAT\n";
    std::cout << "stress : " << stress << "\n";
    std::cout << "statev : ";
    for(unsigned int i = 0 ; i < nstatev ; i++)
        std::cout << statev[i] << "\t" ;
    std::cout << "\n";*/
    
    return OK;
}

///@brief Return elastic stiffness fourth order tensor
///@param dim Dimension of the problem
Tensor4 ElastoplasticMaterial::StiffnessTensor(unsigned int dim)
{
    double mu = young/(2.*(1.+poisson));
    double lambda = (young*poisson)/((1.+poisson)*(1.-2.*poisson));
    if(dim == 3)
    {
        Tensor4 res(3);
        Tensor4 identity;
        identity = identity.Identity(3u);
        Tensor2 delta;
        delta = delta.Delta(3u);
        res = 2.*mu * identity + lambda*res.CrossProduct(delta,delta);
        return res;
    }
	else
	{
		Tensor4 res;
		return res;
	}
}

///@brief Return elastic stiffness matrix
///@param dim Dimension of the problem
Matrix ElastoplasticMaterial::StiffnessMatrix(unsigned int dim)
{
    if(dim == 2)
    {
        ///@warning Works only in plane stress case
        Matrix res(3u,3u);
        res.Nullify();
        res(1,1) = young / (1. - poisson*poisson);
        res(2,2) = young / (1. - poisson*poisson);
        res(3,3) = (young*(1. - poisson)) / (2.*(1. - poisson*poisson));
        res(1,2) = poisson*young/(1. - poisson*poisson);
        res(2,1) = poisson*young/(1. - poisson*poisson);
        return res;
    }
	else
	{
		Matrix res;
		return res;
	}

  
}

///@brief Return elastic compliance fourth order tensor
///@param dim Dimension of the problem
Tensor4 ElastoplasticMaterial::ComplianceTensor(unsigned int dim)
{
    if(dim == 3)
    {
        Tensor4 res(3);
        Tensor4 identity;
        identity = identity.Identity(3u);
        Tensor2 delta;
        delta = delta.Delta(3u);
        res =  ((1.+ poisson)/young) * identity - (poisson/young)*res.CrossProduct(delta, delta);
        return res;
    }
	else
	{
		Tensor4 res;
		return res;
	}

}

///@brief Return elastic compliance matrix
///@param dim Dimension of the problem
Matrix ElastoplasticMaterial::ComplianceMatrix(unsigned int dim)
{
    if(dim == 2)
    {
        ///@warning Works only in plane stress case
        Matrix res(3u,3u);
        res.Nullify();
        res(1,1) = 1. / young;
        res(2,2) = 1. / young;
        res(1,2) = - poisson / young;
        res(2,1) = - poisson / young;
        res(3,3) = 2.*(1. + poisson)/young;
        return res;
    }
	else
	{
		Matrix res;
		return res;	
	}
  
}

///@brief Compute the loading function
///@param sigma Stress tensor
///@param p Cumulated plasticity
double ElastoplasticMaterial::LoadingFunction(Tensor2 sigma, double p)
{
    return sigma.VMEquivStress() - sigmaY - Hiso*pow(p, 1./n);
}

///@brief Compute the loading function
///@param sigma Stress vector
///@param p Cumulated plasticity
double ElastoplasticMaterial::LoadingFunction(StressVector sigma, double p)
{
    return sigma.VMEquiv() - sigmaY - Hiso*pow(p, 1./n);
}
        

///@brief Compute residuals
Vector ElastoplasticMaterial::ComputeResiduals(Tensor2 sigma, Tensor2 Dsigma,
                        double p, double Dp,
                        Tensor2 epsilon, Tensor2 Depsilon,
                        Tensor2 epsilonp)
{
    Tensor4 souplesse = this->ComplianceTensor(3);
    Vector res(7u);

    Tensor2 Rs = (souplesse||(sigma + Dsigma))
                 + epsilonp
                 + (((sigma + Dsigma).Dev())/((sigma + Dsigma).VMEquivStress())) * Dp * (3./2.)
                 - epsilon - Depsilon;
    res[1] = Rs(1,1);
    res[2] = Rs(2,2);
    res[3] = Rs(3,3);
    res[4] = Rs(1,2);
    res[5] = Rs(1,3);
    res[6] = Rs(2,3);

    res[7] = LoadingFunction(sigma+Dsigma, p+Dp);
    
    return res;
}

///@brief Compute residuals
Vector ElastoplasticMaterial::ComputeResiduals(StressVector sigma, StressVector Dsigma,
                                               double p, double Dp,
                                               Vector epsilon, Vector Depsilon,
                                               Vector epsilonp)
{
    Matrix souplesse = this->ComplianceMatrix(2u);
    Vector res(4u);

    Matrix P2(3u, 'P');
    Matrix D(3u, 'D');

    Vector Rs = (souplesse|(sigma + Dsigma))
                 + epsilonp
                 + Dp * (3./2.)*(P2|(D|(sigma + Dsigma)))/((sigma + Dsigma).VMEquiv())
                 - epsilon - Depsilon;
    res[1] = Rs[1];
    res[2] = Rs[2];
    res[3] = Rs[3];

    res[4] = LoadingFunction(sigma+Dsigma, p+Dp);

    return res;
}

///@brief Compute residuals derivatives
Matrix ElastoplasticMaterial::ComputeResidualsDerivatives(Tensor2 sigma, Tensor2 Dsigma,
                        double p, double Dp,
                        Tensor2 epsilon, Tensor2 Depsilon,
                        Tensor2 epsilonp)
{
    // Residuals vector : [Rs_11  Rs_22  Rs_33  Rs_12  Rs_13  Rs_23  Rp]
    // Unknowns vector : [s_11  s_22  s_33  s_12  s_13  s_23  p]
    Tensor4 souplesse = this->ComplianceTensor(3);
    Tensor4 identity;
    Tensor2 delta;
    identity = identity.Identity(3);
    delta = delta.Delta(3);
    double sigmaeq = (sigma+Dsigma).VMEquivStress();
    Tensor2 sigmadev = (sigma + Dsigma).Dev();
    
    Matrix res(7u,7u);

    Tensor4 dRsdDs = souplesse
                     + Dp * (3./(2.*pow(sigmaeq,2.)))
                     * (((identity - (1./3.)*(identity.CrossProduct(delta, delta)))*sigmaeq)
                       - (3./(2.*sigmaeq)) * (identity.CrossProduct(sigmadev, sigmadev))   );

    Tensor2 dRpdDs, dRsdDp;
    
    dRpdDs = (3./2.) * sigmadev / sigmaeq;
    dRsdDp = (3./2.) * sigmadev / sigmaeq;


    res(1,1) = dRsdDs(1,1,1,1);
    res(1,2) = dRsdDs(1,1,2,2);
    res(1,3) = dRsdDs(1,1,3,3);
    res(1,4) = 2.*dRsdDs(1,1,1,2);
    res(1,5) = 2.*dRsdDs(1,1,1,3);
    res(1,6) = 2.*dRsdDs(1,1,2,3);

    res(2,1) = dRsdDs(2,2,1,1);
    res(2,2) = dRsdDs(2,2,2,2);
    res(2,3) = dRsdDs(2,2,3,3);
    res(2,4) = 2.*dRsdDs(2,2,1,2);
    res(2,5) = 2.*dRsdDs(2,2,1,3);
    res(2,6) = 2.*dRsdDs(2,2,2,3);

    res(3,1) = dRsdDs(3,3,1,1);
    res(3,2) = dRsdDs(3,3,2,2);
    res(3,3) = dRsdDs(3,3,3,3);
    res(3,4) = 2.*dRsdDs(3,3,1,2);
    res(3,5) = 2.*dRsdDs(3,3,1,3);
    res(3,6) = 2.*dRsdDs(3,3,2,3);

    res(4,1) = dRsdDs(1,2,1,1);
    res(4,2) = dRsdDs(1,2,2,2);
    res(4,3) = dRsdDs(1,2,3,3);
    res(4,4) = 2.*dRsdDs(1,2,1,2);
    res(4,5) = 2.*dRsdDs(1,2,1,3);
    res(4,6) = 2.*dRsdDs(1,2,2,3);

    res(5,1) = dRsdDs(1,3,1,1);
    res(5,2) = dRsdDs(1,3,2,2);
    res(5,3) = dRsdDs(1,3,3,3);
    res(5,4) = 2.*dRsdDs(1,3,1,2);
    res(5,5) = 2.*dRsdDs(1,3,1,3);
    res(5,6) = 2.*dRsdDs(1,3,2,3);

    res(6,1) = dRsdDs(2,3,1,1);
    res(6,2) = dRsdDs(2,3,2,2);
    res(6,3) = dRsdDs(2,3,3,3);
    res(6,4) = 2.*dRsdDs(2,3,1,2);
    res(6,5) = 2.*dRsdDs(2,3,1,3);
    res(6,6) = 2.*dRsdDs(2,3,2,3);

    res(7,1) = dRpdDs(1,1);
    res(7,2) = dRpdDs(2,2);
    res(7,3) = dRpdDs(3,3);
    res(7,4) = 2.*dRpdDs(1,2);
    res(7,5) = 2.*dRpdDs(1,3);
    res(7,6) = 2.*dRpdDs(2,3);

    res(1,7) = dRsdDp(1,1);
    res(2,7) = dRsdDp(2,2);
    res(3,7) = dRsdDp(3,3);
    res(4,7) = dRsdDp(1,2);
    res(5,7) = dRsdDp(1,3);
    res(6,7) = dRsdDp(2,3);

    ///@bug pow(x, y) return inf if x = 0., even if y has a correct value
    if(p + Dp == 0.)
        res(7,7) = 0.;
    else
        res(7,7) = - Hiso * pow(p + Dp, 1./n - 1.) / n;

    return res;
}

///@brief Compute residuals derivatives
Matrix ElastoplasticMaterial::ComputeResidualsDerivatives(StressVector sigma, StressVector Dsigma,
                                                          double p, double Dp,
                                                          Vector epsilon, Vector Depsilon,
                                                          Vector epsilonp)
{
    // Residuals vector : [Rs_1  Rs_2  Rs_3  Rp]
    // Unknowns vector : [s_1  s_2  s_3  p]
    Matrix souplesse = this->ComplianceMatrix(2);

    double sigmaeq = (sigma+Dsigma).VMEquiv();
    //Tensor2 sigmadev = (sigma + Dsigma).Dev();

    Matrix res(4u,4u);

    Matrix identity(3u, 'I');
    Matrix P2(3u, 'P');
    Matrix D(3u, 'D');

    //Operator that compute the derivative of VM equivalent stress
    Matrix dsigmaeq(3u, 3u);
    dsigmaeq.Nullify();
    dsigmaeq(1,1) = 2.;
    dsigmaeq(2,2) = 2.;
    dsigmaeq(3,3) = 6.;
    dsigmaeq(1,2) = -1.;
    dsigmaeq(2,1) = -1.;

    StressVector dsigmaeqdDsig;   
    dsigmaeqdDsig = (1./(2.*sigmaeq))*dsigmaeq|(sigma + Dsigma);
    
    
    
    Matrix dRsdDs = souplesse + Dp*(3./(2.*sigmaeq*sigmaeq))
                              *(P2|(D|((identity*sigmaeq) - ((sigma + Dsigma)*(dsigmaeqdDsig)))));

    Vector dRpdDs, dRsdDp;

    dRsdDp = (3./(2.*sigmaeq)) * P2|(D|(sigma + Dsigma));
    dRpdDs = dsigmaeqdDsig;

    res(1,1) = dRsdDs(1,1);
    res(1,2) = dRsdDs(1,2);
    res(1,3) = dRsdDs(1,3);


    res(2,1) = dRsdDs(2,1);
    res(2,2) = dRsdDs(2,2);
    res(2,3) = dRsdDs(2,3);


    res(3,1) = dRsdDs(3,1);
    res(3,2) = dRsdDs(3,2);
    res(3,3) = dRsdDs(3,3);


    res(4,1) = dRpdDs[1];
    res(4,2) = dRpdDs[2];
    res(4,3) = dRpdDs[3];


    res(1,4) = dRsdDp[1];
    res(2,4) = dRsdDp[2];
    res(3,4) = dRsdDp[3];


    ///@bug pow(x, y) return inf if x = 0., even if y has a correct value
    if(p + Dp == 0.)
        res(4,4) = 0.;
    else
        res(4,4) = - Hiso * pow(p + Dp, 1./n - 1.) / n;

    return res;
}